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1.  Introduction 

While  there  has  always  been  a  need  to  determine  the  global  position  of  an  underwater 
vehicle,  in  some  missions  involving  search,  mapping,  and  intervention  with  objects, 
navigation  to  local  area  landmarks  is  more  appropriate  and  precise.  All  aspects  of 
autonomous  search  have  been  of  interest  to  us  for  some  time  now,  and  we  have  recently 
developed  and  extended  our  robot  control  system  architecture  using  Prolog  as  a  rule 
based  mission  specification  language  to  drive  vehicle  missions  involving  motion  around 
targets  of  interest.  In  particular,  we  have  studied  the  use  of  onboard  scanning  sonar  to 
perform  local  area  navigation.  Additionally,  we  have  installed  a  new  low  cost  short  /  long 
baseline  acoustic  communications  /  navigation  system  called  DiveTracker,  and  are 
developing  filtering  software  that  would  combine  inputs  from  several  sources  having 
different  update  rates  and  levels  of  precision  to  produce  high  update  rate  navigational 
information  with  the  precision  afforded  by  the  low  update  rate  reference.  Also,  the 
DiveTracker  system  affords  a  low  cost  acoustic  communications  system  that  can  be  used 
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for  low  rate  message  sending  and  retrieval  from  autonomous  vehicles.  In  this  paper,  we 
attempt  to  give  an  elaborate  analysis  of  local  area  maneuvering  using  sonar  based  feature 
detection  from  the  local  scene.  A  mathematical  model  of  the  vehicle  response  is  used  to 
provide  control  inputs  during  periods  when  sonar  updates  are  not  available  and  the 
experimental  results  indicate  that  this  method  will  supplement  other  techniques  where 
positioning  precision  to  centimeters  is  necessary.  In  the  second  and  third  parts  of  the 
paper,  we  outline  some  recent  results  of  our  experiments  in  Monterey  Bay  using  the 
Systran  Donner  Motion  Pak  inertial  system  corrected  by  differential  GPS  when  surfaced. 
The  navigation  system  is  smoothed  and  coordinated  through  a  complementary  filter  that 
bounds  the  normal  drift  in  free  inertial  systems.  Lastly,  some  recent  work  on  an 
examination  of  message  passing  using  IP  protocol  through  seawater  with  the 
'DiveTracker'  system  will  be  discussed. 


2.  Background 

Recent  developments  in  underwater  robotics  are  aimed  at  providing  solutions  to  the 
problems  of  commercial,  scientific,  and  military  missions  in  the  coastal  ocean 
environment.  Small  autonomous  vehicles  will  be  able  to  monitor,  search  and  survey  areas 
of  the  ocean  floor  in  shallow  water.  Providing  results  in  near  to  real  time,  supervised 
autonomous  activity  including  mission  replanning  and  system  reconfiguration  can  be 
used  to  inspect  and  monitor  underwater  structures,  harbor  environments,  and  obtain 
minefield  reconnaissance  data. 


Two  classes  of  mission  arise.  The  survey  mission  requires  an  energy  efficient  vehicle 
to  cruise  and  follow  designated  way  points  whilst  taking  relevant  oceanographic  data. 
The  second  (the  intervention  mission)  requires  a  vehicle  capable  of  slow  speed  and  even 


station  keeping  with  thrusters  and  servo  control  to  objects  using  vision,  sonar,  tactile 
sensors,  or  combinations  thereof.  Examples  of  survey  vehicles  include  the  Odyssey  [1], 
and  the  Ocean  Voyager  [2],  Remus  [3]  and  the  larger  vehicles  such  as  the  Draper  UUV 
[4]  and  the  Navy's  LDUUV  [5],  while  examples  of  thruster  controlled  vehicles  include 
the  OTTER  [6],  the  Phoenix  [7],  the  Marius  [8]  and  Vortex  [9],  and  the  entire  class  of 
Remotely  Operated  Vehicles  called  ROV's  [10]. 

In  the  class  of  vehicles  designed  for  the  intervention  mission,  Marks,  et.  al.  [6]  have 
studied  the  problem  of  servo  positioning  the  OTTER  vehicle  to  visual  cues  from 
stereoscopic  cameras  although  monocular  video  data  was  used  to  perform  edge  detection 
and  servo  control  of  the  pan  and  tilt  mounting  coupled  to  the  vehicle  platform.  Some  of 
the  co-authors  of  this  paper  have  reported  positioning  control  of  the  Phoenix  vehicle  to 
acoustic  returns  from  high  frequency  (1.2Mhz.)  sonar  where  the  sonar  was  integrated  into 
the  execution  level  control  software  [11]  as  necessary  to  the  stabilization  of  the  vehicle 
motion.  Part  of  the  problem  lies  in  the  need  for  improved  modeling  of  thruster  behavior 
as  described  in  Yoerger  [12],  and  Healey  et.  al.  [13].  Once  maneuvering  control  around 
objects  in  the  local  area  scene  is  understood  to  a  satisfactory  degree,  intervention  using 
manipulators  and  other  tactile  devices  will  be  enabled.  Such  activities  as  changing  out  a 
battery  pack  for  a  bottom  mounted  sensor  or  finding  and  entering  an  underwater  garage 
for  repowering  will  then  become  commonplace. 

We  focus  first  on  the  problem  of  local  area  navigation  and  maneuvering  using  model 
based  control  and  acoustic  feature  extraction  techniques  for  precise  positioning. 


3.  Model  Based  Control  Formulation 


Absent  of  an  inertial  position  reference,  where  sonar  position  updates  are 
asynchronous,  and  occur  at  times  much  longer  than  the  control  frequency  of  10  Hz,  a 
dynamic  model  of  the  vehicle  is  used  for  state  information  between  updates.  A  three 
degree-of-freedom  model  (longitudinal,  lateral,  and  heading)  is  used  since  the  motion  is 
restricted  to  the  horizontal  plane  with  the  depth  maintained  by  a  separate  controller.  The 
model  is  given  by  including  drag,  added  mass,  and  steady  state  thrust  and  for  surge  is 

M  xu(t)  +  bxu(t)\u(t)\=2axvx(t)\vx(t)\  (1) 

The  sway  directional  equation  of  motion  is 

My  v(t)  +  byv(t)\v(t)\  =ayvb„  (0 1 vw,  p)|  +avvs„  (t)|vj/(  (t)|  (2) 

and  finally  the  yaw  equation  of  motion  becomes 

I,  r{t){ t)  +  \r(t  )\r(  t  )|  =av  \blt  it) |vw,  (f)|  -av  vslt  (t) \vslt  (f)|  (3) 


where 


Mx  =  m  +  max  ,  My=m  +  may  ,  L  =  1  zz  + 1 a7Z 


and 


v*(0|v,  (0|  =  (vb  (0  |vb  (f)|  +  Vra  (0  |vra  (t  )|)/2 

m  is  the  vehicle  mass,  I7Z  the  mass  moment  of  inertia  about  the  body-fixed  z  -axis,  and 
the  subscript  " a "  refers  to  the  added  mass  or  inertia  of  the  body.  u(t) ,  v(t) ,  and  r(t)  are 
the  body-fixed  rates  for  longitudinal  (x-axis),  lateral  ( v  -axis),  and  heading  (\j/ ) 


directions.  bx ,  by,  bv  are  the  square-law  damping  coefficients,  vis.  (?) ,  v  ra  (7) ,  and  vWr(?) , 

vs./r  (?)  are  the  thruster  motor  input  voltages  for  the  left/right  rear  screws,  and  the 

bow/stem  lateral  thrusters  respectively.  The  voltage  to  force/moment  coefficients  are 
given  by  ax ,  ay  ,  and  av  . 

The  above  dynamics  equations  can  be  formulated  using  matrix  notation  as 

Mx(t)=f(x(t),b)  +  g(a)u(t)  (4) 

and  vehicle  kinematics  are  defined  by 

z(t)=h(Mf  )x(t)  +  uc(t).  (5) 

The  body-fixed  rates  are 

x(t)={u(t)  v(? )  r(t)Y , 

and  the  global  position  and  orientation  is  given  by 

z(t)={X(t)  Y(t)y(t)f . 

The  vector  describing  the  hydrodynamic  drag  that  is  a  function  of  the  body-fixed  rates 
and  square-law  damping  coefficients,  b={bx  by  b,v  }  is 

f(  x(  t  ),b  )  =  {-  bxu{t)\u(t)\  -  byv(t)\v(t)\  -\r(t  )\r(  t  )|f , 


the  mass  matrix  is 


0  0 


M  = 


Mx 

0 

0 


M  y  0 
0  I. 


and  input  gain  matrix  which  is  solely  a  function  of  the  thruster  coefficients, 
a  ={  a  t  av  av  /  is 


2a 


X 


g(  a)= 


0 

0 


0  0 
av  ay 
—  a,^ 


Finally,  the  control  input  vector  is  defined  as 


u(t)={vx(t)vx(t)  ybu(t)ybu(t)  ysi,(t)yslt(t)\Y . 


(6) 


For  the  case  of  translation  in  X,  Y  and  rotation  \| / ,  the  transformation  matrix  from  the 
body-fixed  axes  to  the  global  reference  is  given  by 


h(v?(t))  = 


COifXI/f  t  )) 
sin(\\i(  t )) 


0 


-sin(\\f(t))  0 
cos(y\i(t ))  0 

0  1 


(7) 


and  it's  time  derivative  is 


h(y(OSY(t))= 


— \|/  ( t  )sin(\ \f(  1 )) 
\\f(t  )cos(y(t )) 


0 


-\|/(  t  )cos(\\i(t ))  0 
-\|/(7  )sin(\\i(t ))  0 
0  0 


Any  current  disturbances  are  represented  by 


uc(t)={  uc(t)  vc(t)  rc(t)  }T 


where  the  elements  uc(t)  are  the  body-fixed  current  rates. 


The  sliding  mode  control  law  can  now  be  formulated  defining  the  tracking  error 
vector  in  terms  of  global  coordinates  as 


z(t) 

7  ( t  ) 

z(t) 

z(t)_ 

7  ( t  ) 

_**com '  '  _ 

_z(  t )_ 

The  subscript  "com"  refers  to  the  commanded  value  of  the  position  or  rate  in  question, 
where  commanded  time  variations  of  states  must  be  consistent  with  vehicle  physical 
capabilities  and  usually  come  from  separate  path  planning  algorithms. 


Since  the  dynamics  equation  is  in  terms  of  body-fixed  rates  and  accelerations, 
Equation  (8)  can  be  expressed  in  terms  of  body-fixed  rates  using  Equation  (5).  If  uc(t)  is 

assumed  zero: 


h(\\i(t))x(  t) 

Hv(t))xcom(t) 

h(\\i(t))x(t) 

z(f) 

^  com  ( t  ) 

z(f) 

Now  that  the  tracking  error  has  been  formulated,  an  equation  defining  the  sliding 
surface  in  terms  of  this  error  can  be  written  as 


o(x(t),z(t))=[Sl 


S2 


h(y(  t))x(t) 
z(t) 


(10) 


where 


o(x(t),z(t))e'R3xl  ;  S],S2g<R3x3 


The  elements  of  S;  and  S2  can  be  selected  to  provide  the  desired  performance  of  the 
closed  loop  system.  For  the  case  of  planar  control,  these  become 


T 

0 

o" 

X 

0 

0  " 

Si  = 

0 

l 

0 

s2  = 

0 

0 

0 

0 

t 

0 

0 

To  ensure  that  stable  tracking  behavior  is  achieved,  the  condition: 


lim 


-a(  x(  t),z(  t  ))—>0 


with 


lim 
t  — »  °° 


a(  x(  1  ),z(t ))  — >  0 


will  also  imply 


x(t ),z(t  )->0  as  t-> °o . 


The  condition  that  a(x(t),z(t))  is  always  decreasing  can  be  established  if  a  Lyapunov 
function  of  the  sliding  surface  is  formed  as 


V(t)  = 


(x(t),z(t))*a(x(t),z(t)). 


(11) 


and 


V=6'(x(t),z(y))*o(x(t),z(y)) 


(12) 


Global  asymptotic  stability  is  guaranteed  if  V(t )  is  positive  definite  and  V(t)  is  negative 
definite.  The  quadratic  nature  of  (11)  assures  the  positive  definiteness  of  V(t),  while 
negative  definiteness  of  V(t )  may  be  met  by, 

6i(x(t),z(t))=-r\isgn(oi(x(t),z(t)))  i  =  x,y,\\i  (13) 

where  each  t|,  is  a  positive  scalar  matched  with  each  control  direction,  x ,  y  ,  and  \|/ .  The 
positive  definiteness  of  V(t )  and  the  negative  definiteness  of  V(  t ) ,  implies  that  given  any 
initial  condition,  a(x(  0),z(0)),  a(x(t  ),z(t ))  will  remain  bounded  such  that 


V(a(x(t),z(t))<V(a(x(0),z(0)). 


Since  sgn(a t( x(t ),z(t )))  is  discontinuous  across  a( x( t ),z(t ))  =  0,  undesirable  switch 

chattering  can  occur.  This  is  alleviated  by  the  use  of  a  "boundary  layer"  around  zero. 
Therefore,  instead  of  using  a  sgn  function,  a  continuous  form  is  preferred  such  that 


sat(at(  x(t),z(  t  ))./<|>(-  )  = 


Ugn(Oi(x(t),z(t))) 
}a,f  x(t),z(t))/( |), 


if\Oi(  x(t),z(t))\>§i 
otherwise 


where  "./"  denotes  element  by  element  division.  Another  approach  is  to  simply  use  the 
continuous  function  tanh(a(x(t),z(t))).  Substituting  the  definition  of  sat  into  Equation 

(13)  and  noting  Equation  (10),  it  can  be  written  in  a  more  compact  form  as 

c(x(t),z( t))  =  S,h(\\i( t))x(t)  +  SIhfyf(  t))x(  t )  +  S2z(t  )  =  -F(o(  x(t  ),z( t )),§  )  (14) 

Substituting  the  dynamics  equation  (1)  into  (14)  yields  the  control  solution  u(t)  and 
since  the  matrices  /(•),  g(  • ) ,  and  h(  • )  are  uncertain  in  general,  they  must  be  formulated 


using  estimates,  denoted  as  /(•),§(•),  and  h(»),  where  the  (•)  is  used  for  notational 
compactness.  The  control  vector  can  be  split  into  three  parts 

u(t  )  =  ul(t  )+u2(t  )+u3(t ) 


where 


ul(t)=gC)~1(Mxcom(t  )-/(•)) 


(15) 


contains  the  acceleration  terms, 


u2(t  )=g(Tl  Mh~lC)(iiC)hC)-l  +  S1-1S2)t(t)  (16) 

contains  the  velocity  terms,  and  finally 


u3(t)  =  gC)-IM-1C)S1-1F(oC),<b) 


(17) 


is  the  switching  term,  where 


xcom(t)  =  h  V •)(~h( •)&(•)  1zcom(t)  +  zcom(t)) 

Sj  is  identity,  and  if  all  commanded  velocities  and  accelerations  are  zero  the  control 
reduces  to 


«if t)=g(»)  lf(») 

u2(t)=-gc)-1MhTcdc)hTc)+s2  )z(t) 
u3(t)  =  gerIMhTC)F(aC )#) 


(18) 


where  ux(t),  u2(t),  and  u3(t)  contain  the  acceleration,  velocity  and  switching  terms 
respectively. 

4.  Target  Detection  with  Sonar 

To  perform  local  area  navigation  using  sonar,  it  is  necessary  to  select  an  easily 
discernible  feature  in  the  vehicle  operating  area  and  use  it  as  a  fixed  reference.  The  target 
feature  should  be  stationary  and  reasonably  unique  with  respect  to  other  structures  in  the 
sonar  field  of  view.  This  will  be  necessary  to  enable  repeatable  and  unambiguous 
detection  of  the  reference  feature.  In  order  to  classify  these  features,  each  must  be 
segmented  into  a  separate  object  and  analyzed  to  see  if  it  posses  the  structural  properties 
of  the  desired  target  for  reference. 

For  the  results  presented  in  this  paper,  the  target  used  for  the  local  navigation 
reference  was  a  0.5  meter  diameter,  0.75  meter  long  cylinder  placed  vertically  in  the 
water  column  of  the  NPS  AUV  test  tank  which  measures  6.0  by  6.0  meters  square  and 
1.8  meters  deep.  A  Tritech  ST1000  profiling  sonar  head  was  used  which  is  mounted 
vertically  in  the  nose  of  the  NPS  Phoenix  vehicle.  The  head  uses  a  stepper  motor,  which 
can  mechanically  rotate  the  transducer  through  360°  with  respect  to  it's  mounting  at  a 
minimum  angular  resolution  of  0.9°.  For  each  step,  the  sonar  is  pinged  and  a  single  range 
value  is  returned  which  enables  a  complete  profile  of  the  area  surrounding  the  vehicle  to 
be  constructed. 

An  actual  scan  of  the  cylindrical  target  and  square  tank  walls  is  shown  in  Figure  2.  A 
sweep  width  of  ±35°  and  angular  resolution  of  1.8°  was  used.  Each  dot  or  "pixel" 
corresponds  to  a  discrete  range  value  returned  by  the  sonar  for  a  given  angular  position  of 


the  transducer  head.  Several  disjoint  groups  or  segments  of  pixels  are  visible  in  the  field 
of  view:  the  two  sections  of  the  tank  wall,  and  the  cylinder  which  casts  an  acoustic 
shadow  against  the  wall.  Since  sonar  range  drop  outs  and  noise  are  common  with  sonars, 
the  tank  wall  to  the  upper  right  of  the  cylinder  is  broken  up  into  several  segments, 
although  in  reality,  it  is  a  continuous  feature.  It  is  this  nature  of  acoustic  sensors  that  lead 
to  the  development  of  the  following  algorithms  for  cylinder  detection  in  the  NPS  test 
tank. 

Since  the  cylinder  is  the  desired  target  for  the  local  area  reference,  returns  from  the 
tank  walls  need  to  be  filtered  out  and  ignored.  This  can  be  accomplished  by  segmenting 
each  contiguous,  disjoint  group  of  range  pixels  and  analyzing  them  for  the  desired 
characteristics  of  a  cylinder.  The  method  to  isolate  these  segments  is  outlined  in  the  flow 
diagram  in  Figure  3.  The  filter  is  initialized  by  pinging  at  a  fixed  bearing  to  obtain  an 
average  range  value,  F  .  Once  this  is  done,  the  head  is  commanded  to  scan  in  a  clockwise 
direction  and  each  range  return  is  first  tested  for  feasibility.  If  the  range  is  zero  or  if  it 
exceeds  the  maximum  operating  range,  rmax ,  it  is  ignored  and  that  range,  r; ,  is  set  to  the 

current  average  range,  F,  and  the  scan  proceeds.  If  the  range  is  feasible,  a  test  is 
performed  to  see  if  it  lies  within  an  error  band  of  ±  A r  of  the  average  and  if  so,  the  value 
of  F  is  recalculated  using  the  new  range.  The  range  and  the  associated  bearing  angle  is 
then  stored  in  a  vector  of  size  n  ,  (the  number  of  pixels  defining  the  segment).  If  the  range 
falls  outside  of  the  error  band,  a  flag  is  set  to  examine  how  closely  subsequent  returns 
compare  to  the  new  range.  A  secondary  average,  rnew ,  is  initialized  to  this  value  and  a 
new  segment  is  declared  if  the  next  nmm  adjacent  ranges  are  consistent  with  this  average 
at  which  time  the  current  average  is  set  to  rnew .  The  old  segment  is  now  terminated  at 
i  -  nmin  and  the  range,  bearing  and  pixel  count  values  are  processed  to  extract  any  shape 
information  they  may  provide.  If  the  subsequent  ranges,  less  than  nmin  pixels  are 
inconsistent  with  rnew ,  and  fall  near  the  previous  average,  a  new  segment  is  not  assumed 


and  the  scan  continues  using  r .  These  "false  alarms"  occur  quite  frequently  due  to  the 
nature  of  the  sonar  returns  which  contain  drop  outs  and  false  ranges.  The  value  of  nmitl 

can  be  varied  depending  on  the  environment  of  operation.  For  the  test  tank  which 
provides  relatively  clean  signals,  the  value  of  nmm  is  typically  3,  but  in  more  noisy 

conditions,  a  larger  value  should  be  used  to  provide  higher  filtering. 

Once  a  separate  segment  has  been  identified,  the  vector  containing  it's  ranges  and 
bearing  angles  is  analyzed.  The  flow  diagram  for  this  algorithm  is  shown  in  Figure  4.  To 
determine  if  the  object  defined  by  the  segment  is  a  cylinder,  it  must  posses  the  following 
characteristics: 

1.  Consist  of  a  sufficient  number  of  pixels,  n ,  that  does  not  exceed  a 
maximum,  nmax .  If  the  number  of  pixels  is  large,  in  this  case  greater  than  10  it 

can  be  safely  assumed  the  segment  is  a  wall  due  the  relative  size  of  the 
cylinder. 

2.  Be  in  front  of  the  tank  walls.  This  is  an  obvious  observation  since  the 
cylinder  is  assumed  to  be  in  the  tank  but  must  be  included  in  the  algorithm  to 
avoid  confusion  by  perceived  cylindrical  shaped  areas  on  the  wall  due  to 
noise. 

3.  Have  a  central  range  closer  than  it's  edges.  Since  a  cylinder  appears  the 
same  from  any  direction  in  a  horizontal  plane,  the  center  of  the  segment  will 
always  be  closer  the  sonar  than  the  beginning  and  ending  edges. 

The  preceding  algorithms  have  been  used  with  much  success  in  the  NPS  test  tank  and 
should  operate  well  in  an  open  water  environment  especially  since  the  tank  walls  will  be 


absent  and  the  reference  target  the  most  visible  object  in  the  area.  This  procedure  can  be 
modified  to  search  for  other  geometric  shapes  since  the  idea  of  segmentation  of  each 
feature  is  retained  but  does  not  attempt  to  supplant  more  sophisticated  and  robust  pattern 
recognition  algorithms  available.  This  method  was  adopted  since  it  can  be  executed  in 
real  time  and  is  simply  used  as  a  means  to  perform  the  tasks  described  in  the  following 
sections. 

5.  Relative  Position  Estimation 

Once  the  reference  target  has  been  identified,  it  becomes  the  origin  of  the  navigation 
coordinate  frame  where  the  X  -axis  is  aligned  with  heading  0  degrees  and  the  Y  -axis 
along  a  heading  of  90°  as  shown  in  Figure  5.  The  two  dimensional  position  vector  to  the 
origin  of  the  vehicle  body-fixed  reference  with  respect  to  the  navigation  frame  at 
detection  time  T  is 


RV(T) 


\*JT)  1 
1 YJT)  J 


(rs(T)  +  Rcyl(T)) 


(19) 


where 


rs(T)  =  h(y(T))- 


& 


and  x, ,  vv  is  the  position  of  the  sonar  head  in  vehicle  coordinates. 


J (Rcyl  (O  +  rcyl  >cos( ¥  CO  +¥,  (O  /I 
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where  Rcy,(T)  is  the  sonar  range  to  the  target,  v| t  S(T)  is  the  heading  angle  of  the  sonar 
beam,  and  for  the  case  of  a  cylindrical  target,  rcyl  is  it's  radius.  After  the  target  has  been 

found,  and  the  location  of  the  vehicle  is  determined,  the  sonar  is  commanded  to  sweep 
across  it  at  a  prescribed  angular  sweep  width  denoted  \\ rsw  about  a  heading  which  is  the 

center  of  the  target.  This  reduces  the  amount  of  delay  time  between  re-acquiring  the 
target. 

6.  Position  Update 

Since  there  is  a  delay  time  of  up  to  10  seconds  between  successive  target  detection, 
the  vehicle  control  must  use  a  dynamic  model  between  position  updates.  Equation  4  is 
integrated  to  obtain  estimates  of  the  vehicle  position  denoted  X(t),  and  Y(t )  during  this 
time.  The  scan  direction  command  angle  \|/vrf  (t)  between  position  updates  is  computed 
using 


t  )  =  atan2 


(  -(  Y(t )  +  xssin(\\f(  t ))  +  y scos(\f(  t )) )  ^ 
-(  X(  t  )  +  xscos('ty(  t ))  + yssin(\\i(  t ))  ) 


(21) 


and  a  maneuver  using  this  approach  is  shown  in  Figure  6.  If  the  target  has  not  been 
reaquired  within  a  specified  time,  the  head  is  commanded  to  return  to  continuous  sweep 
mode.  This  is  needed  if  the  scan  width  is  too  narrow  and  there  exists  a  large  discrepancy 
between  the  model  and  the  actual  vehicle,  the  scan  direction  calculated  from  the  estimates 
of  position  can  be  in  error.  One  approach  to  reduce  this  possibility  is  to  increase  the  scan 
width,  vj/  sw  to  say  120°  degrees  but  doing  this  will  increase  the  time  between  updates  and 

has  not  been  done  for  this  series  of  experiments. 


For  vehicle  control  in  a  plane,  the  complete  state  is  defined  by 


X(t)  =  {u(t)v(t)r(t)X(t)Y(t)y(t)}r 


and  the  block  diagram  representation  of  the  control  scheme  is  shown  in  Figure  7.  When 
the  cylinder  has  been  identified,  the  model  is  asynchronously  updated  at  time  of  target 
detection  using  a  Kalman  filter  of  the  form 


X(T)  =  (l-K)X~(t)  +  KXv(T) 


Y(T)  =  (l-K)Y~(t)+KYv(T) 


(22) 


where 


K  = 


_  2  .  _  2 

o,„+o. 


(23) 


and  a  l  is  the  variance  of  the  system  model  estimate  of  position  and  a  (  is  the  variance  of 
vehicle  position  using  the  sonar.  X~(t),  and  Y~(t)  is  the  current  estimate  of  position 
from  the  model  just  before  the  correction  from  the  sonar  is  obtained.  This  analysis 
assumes  the  position  estimate  from  the  sonar  is  extremely  accurate  and  the  model  very 
inaccurate.  Therefore,  the  variance  for  position  from  sonar  is  set  to  0  and  infinity  for  the 
model.  This  causes  the  current  estimate  from  the  model  to  be  disregard  at  the  time  of 
sonar  update  and 

reduces  Equation  (22)  to  be  simply 


X(t)=KXv(t) 


(24) 


Y(t  )  =  KYV( t ) 


which  states  complete  confidence  in  the  sonar.  At  this  time  the  dynamic  model  of  the 
system  is  reset  to  the  values  obtained  from  Equation  (24)  and  the  model  updates  from 
there  during  the  next  interval  between  updates. 


The  onboard  gyroscopes  provide  the  heading  angle  and  yaw  rate  values  at  10  Hz, 
which  are  synchronous  and  highly  accurate  and  no  estimation  of  these  is  required.  The 
observation  vector  is  defined  by 


y(t)=CX(t)  (25) 


where  the  observation  matrix  is 

“ 0  0  0  0  0  0 

0  0  0  0  0  0 

0  0  1  0  0  0 

c  = 

0  0  0  0  0  0 

0  0  0  0  0  0 

0  0  0  0  0  1 


With  only  these  two  measurements  made,  Equation  (25)  reduces  to 


y(t)= 


(26) 


which  is  used  each  time  step  in  the  vehicle  controller  and  dynamic  model. 


7.  Experimental  Results 


A  five  pose  experiment  was  performed  in  the  NPS  hover  tank.  During  execution,  all 
pertinent  data  was  collected,  including  depth  and  heading  information,  all  sonar  data,  and 
the  estimates  of  position,  position  rate,  and  the  updates  from  the  sonar.  Table  1  shows  the 
commanded  position  and  heading  comprising  the  five  poses  and  are  shown  in  Figure  8. 


Table  1.  Commanded  ] 

Mission  Poses 

Pose 

X  (m) 

YCOm  (m) 

(rad) 

1 

2.13 

-2.13 

0.0 

2 

2.13 

0.0 

0.0 

3 

2.13 

2.13 

0.0 

4 

2.13 

0.0 

0.5236 

5 

-2.74 

-2.13 

0.0 

The  following  tables  give  the  parameter  values  used  in  the  vehicle  model  and  the 
sliding  mode  controller  gains. 


Table  2.  Parameters  for  Vehicle  Model 


Parameter 

Value 

Unit 

m 

194.88 

Kg 

max 

19.49 

Kg 

may 

155.90 

Kg 

53.60 

Kg-m2 

I zza 

53.60 

Kg-m2 

bx 

63.80 

Kg/m 

by 

815.46 

Kg/m 

\ 

74.86 

Kg-m2 

ax 

0.056 

N/V2 

ay 

0.018 

N/V2 

av 

0.008 

N-m/V2 

Table  3.  Sliding  Mode  Controller  Gains 


Parameter 

Value 

Unit 

K 

0.20 

rad/sec 

K 

0.20 

rad/sec 

\ 

0.20 

rad/sec 

hv 

0.15 

m/sec2 

hy 

0.09 

m/sec2 

hv 

0.20 

rad/sec2 

0.06 

m/sec 

0.09 

m/sec 

<l»v 

0.20 

rad/sec 

Note:  aslt  =ablt  =ayllt ,  where  l„  is  the  distance  from  the  mass  center  of  the  vehicle  to  the 
center  of  the  lateral  thruster  axes  which  is  the  same  for  both  thrusters. 

The  experiment  specified  the  vehicle  to  submerge  to  a  depth  of  0.4  meters  using 
vertical  thrusters  as  detailed  in  [14].  Once  this  depth  was  reached,  the  ST1000  sonar  was 
activated  and  scanned  clockwise  until  the  target  (cylinder)  was  identified.  At  this  time, 
the  first  pose  (1)  was  issued  and  the  vehicle  started  the  controlled  maneuver. 

Most  control  phase  transitions  of  the  Phoenix  are  event  based,  meaning  that  a  certain 
set  of  criteria  must  be  met  in  order  for  a  transition  to  occur.  A  common  example  of  this  is 
when  a  position  set  point  is  sent  to  the  vehicle  controllers  and  reached.  A  method  of 
determining  whether  the  vehicle  has  indeed  reached  this  point  must  be  programmed  into 
the  control  logic.  Measuring  the  position  error  alone  and  declaring  the  maneuver 
complete  when  this  error  is  small  is  not  sufficient.  This  is  because  the  vehicle  could  be 
overshooting  the  commanded  position  and  simply  passing  through  the  set  point. 
Therefore,  not  only  must  the  position  error  be  small  but  the  rate  error  must  also  be  small. 
This  dual  criteria  can  be  expressed  mathematically  as  a  positive  definite,  linear 
combination  of  the  position  error  e  and  the  position  rate  error  e  ,  such  that 

>  =  wc|e|  +  Wj,\e\  (27) 

where  we  and  w $  are  weights  for  the  position  and  rate  errors  respectively  and  for  planar 


motion  the  errors  are 


fv  _  V  ] 

com  v 

Xcor,,-* 

e  =< 

Y  -Y 

com  v 

•  and  e = - 

Yearn-? 

V  com  -V  , 

r  —  r 

com 

This  equation  allows  a  minimum  value  of  a  for  each  control  direction  X ,  Y ,  and  \\> 
denoted  ,aox ,  o0Y,  and  a,A|;  to  be  specified  defining  a  threshold  for  the  combination  of 

errors  which  can  be  set  relatively  large  when  precision  control  is  not  required  or  low  for 
extremely  precise  positioning.  Once  each  a  drops  below  it's  respective  a0 ,  the  maneuver 

is  declared  complete  and  a  transition  to  the  next  control  phase  may  occur. 

For  these  experiments,  the  transition  was  based  on  position  errors  from  the  sonar 
ranges,  not  from  the  model  estimates  since  the  model  will  always  predict  a  very  smooth 
trajectory  to  the  set  point.  The  parameters  for  the  error  equation  used  were  oox  =  a0Y  = 
0.08  meters,  cr^  =0.1  radians,  and  we  =  wt  =  1.0. 

Figure  9  shows  the  position  response  results  where  the  upper  trace  is  Y(t)  and  the 
lower  X(t) .  The  position,  calculated  from  sonar  at  update,  XJT )  and  YV(T )  are  shown 
with  circles  and  asterisks  respectively.  Examining  the  response  for  X(t)  it  is  evident  that 
the  model  for  the  longitudinal  direction  is  in  error  since  the  predicted  position  at  the  time 
of  correction  is  about  double  that  calculated  with  the  sonar.  This  mismatch  has  been 
attributed  to  the  absence  of  shrouds  around  the  rear  screws.  Without  them,  an  unmodeled 
transient  force  lag  is  present  that  is  common  with  open  propellers.  Since  this  lag  was 
uncompensated,  and  the  control  was  dictated  by  the  model  predictions  between  position 
updates,  large  voltage  commands  to  the  screws  were  of  too  short  a  duration  to  build  up 
sufficient  force  on  the  vehicle  as  shown  in  Figure  10.  The  performance  was  further 
degraded  by  the  estimated  position  and  rate  feedback  from  the  model.  As  these  values 
were  assumed  to  be  nearing  the  set  point  pose,  the  controller  actually  reversed  the 


propellers  (negative  voltage  command)  in  an  attempt  to  slow  the  vehicle.  This  effect  can 
also  be  clearly  seen  in  Figure  10  between  the  time  46.1  seconds  and  53.1  seconds,  the 
time  of  the  position  update  from  the  sonar.  The  prediction  of  the  lateral  movement,  Y(t) , 

is  much  better  since  the  cross-body  thrusters  are  shrouded  due  to  their  tunnel  design  and 
the  model  parameters  are  well  established.  Model  /  sonar  update  differences  were 
attributed  to  an  unknown  current  that  was  identified  between  successive  updates  This 
allows  compensation  for  model  errors  and  external  disturbances.  In  all,  local  dynamic 
positioning  to  centimeters  is  possible  in  the  tank  environment. 

8.  GPS  /  INS  Navigation 

Another  aspect  of  our  work  relates  to  precise  underwater  and  near  surface 
navigation  of  AUVs  using  a  combination  of  inertial,  acoustic,  and  GPS  navigation.  In 
order  to  carry  out  such  research  in  a  realistic  context,  we  have  constructed  an 
instrumented  tow-fish  containing  a  small,  low  cost  inertial  measurement  unit  (EMU)  and  a 
GPS  antenna  [15].  To  date,  data  collected  in  Monterey  Bay  suggests  that  submerged 
navigation  for  periods  of  several  minutes  is  possible  with  less  than  10  meters  rms.  error 
using  only  water  speed  sensing  and  a  magnetic  compass  to  aid  pure  inertial  navigation. 
We  have  also  shown  that  when  on-line  error  estimates  indicate  the  need  for  inertial  reset, 
this  can  be  accomplished  to  a  precision  of  about  1  meter  rms.  This  precision  is  obtained 
using  locally  derived  differential  GPS  even  in  moderate  sea  states  with  an  antenna 
extending  only  a  few  inches  above  the  ocean  surface  [16].  Further  at  sea  testing  of  this 
system  will  be  reported  in  [17].  This  work  will  investigate  further  accuracy 
improvements  attainable  by  a  low  cost  small  size  acoustic  Doppler  measurement  of 
velocity  over  ground  or  with  respect  to  the  water  column.  By  further  developments  in 
Kalman  filtering  for  these  small  units  that  is  based  on  complementary  high  and  low 
frequency  filters  we  would  expect  that  both  high  frequency  updates  with  low  frequency 


corrections  will  allow  compensation  of  disturbances  induced  by  wave  motion  for 
precision  positioning  and  global  precision  to  DGPS  accuracy. 


9.  Acoustic  Communications  and  Navigation 

Recently,  the  Phoenix  AUV  has  undergone  a  hardware  upgrade  to  enable  open 
water  testing  in  the  Moss  Landing  harbor  facilities  of  MBARI.  This  upgrade  involved 
adding  differential  GPS,  an  acoustic  short  /  long  baseline  navigation,  communications 
and  tracking  system  (Dive-Tracker),  and  a  Sun  Voyager  notebook  workstation  as  a 
second  internal  vehicle  computer.  The  Sun  Voyager  processes  the  Strategic  and  Tactical 
level  software  while  linkage  to  the  Execution  level  software  employs  internal  ethernet 
socket  communications.  These  changes  have  allowed  us  to  move  from  our  fresh  water 
test  pool  to  a  more  useful  and  realistic  salt-water  environment.  Results  obtained  from 
experiments  in  salt  water  to  date  have  shown  that  it  is  indeed  possible  to  send  IP  protocol 
communications  through  seawater  using  the  Dive-Tracker  system  [18],  even  though  the 
data  rate  and  distance  are  not  large.  Also,  for  local  area  positioning,  the  Phoenix  has  used 
a  short  baseline  technique  in  the  Moss  Landing  harbor  area,  giving  precision  to  tens  of 
centimeters  with  range  to  over  100  m.  These  results  will  be  reported  in  six  M.S.  theses  to 
be  completed  by  NPS  students  in  March  1996,  and  summarized  in  [19]. 


10.  Discussion  and  Conclusion 

The  results  of  these  experiments  have  shown  that  it  is  possible  to  navigate  an 
underwater  vehicle  in  a  local  area  using  an  acoustic  sensor  for  position  information.  The 
accuracy  of  the  model  used  between  updates  is  moderately  satisfactory  and  can  allow  for 
time  varying  currents.  However,  some  additional  model  adjustments  could  be  made  to 


compensate  for  the  force  lag  in  the  longitudinal  direction  during  transient  thrust 
conditions.  This  undesirable  effect  could  also  be  alleviated  by  the  addition  of  shrouds 
around  the  rear  screws  which  should  bring  the  performance  up  to  that  of  the  lateral 
thrusters.  While  these  results  were  taken  in  a  tank  environment,  another  improvement 
would  be  to  fuse  the  model  with  an  INS  system  in  between  updates  from  the  sonar  and 
then  fuse  that  estimate  with  the  sonar  data  to  obtain  a  smoother  averaging  at  update  time. 
This  would  allow  for  compensation  of  wave  induced  disturbances,  while  retaining  the 
positioning  precision  found.  Since  the  sonars  are  mechanically  scanned,  and  a  delay  of  up 
to  10  seconds  between  position  update  is  common,  use  of  an  electronically  scanned  or 
multi-beam  sonars  may  be  preferable  although  our  experience  to  date  has  been  that  cross¬ 
talk  between  beams  can  be  a  serious  problem. 
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